/*
* Nettle Source File.
* Copyright (C) 2013, Nettle (http://nttl.ru/).
*
* This file is part of NettleMath.
* All rights reserved. This program and the accompanying materials
* are made available under the terms of the GNU Lesser General Public License
* (LGPL) version 2.1 which accompanies this distribution, and is available at
* http://www.gnu.org/licenses/lgpl-2.1.html
*
* This library is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
* Lesser General Public License for more details.
*
* Description:
*
* History:
* Created by Sergey Serb, l-proger@yandex.ru
*/

#ifndef NQuaternion_h__
#define NQuaternion_h__

#include "NVector3.h"
#include <cmath>

namespace NettleMath{
    template<typename T>
    struct NQuaternion{
        T x;
        T y;
        T z;
        T w;

        NQuaternion()
        {
        }

        NQuaternion(const T& qx, const T& qy, const T& qz, const T& qw)
            : x(qx), y(qy), z(qz), w(qw)
        {
        }

        NQuaternion(const T v[])
            : x(v[0]), y(v[1]), z(v[2]), w(v[3])
        {
        }

        NQuaternion(const NQuaternion& q)
            : x(q.x), y(q.y), z(q.z), w(q.w)
        {
        }

        NQuaternion(const T& angle, const NVector3<T>& axis){
            setAngleAxis(angle,axis);
        }


        NQuaternion operator -() const{
            return NQuaternion(-x, -y, -z, -w);
        }

        bool operator == (const NQuaternion& right) const{
            return ((x == right.x) && (y == right.y) && (z == right.z) && (w == right.w));
        }

        bool operator != (const NQuaternion& right) const{
            return (x != right.x) || (y != right.y) || (z != right.z) || (w != right.w);
        }
        NQuaternion operator *(float other) const{
            return NQuaternion(x * other, y * other, z * other, w * other);
        }
        NQuaternion operator +(const NQuaternion& other) const{
            return NQuaternion(x + other.x, y + other.y, z + other.z, w + other.w);
        }
        NQuaternion operator *(const NQuaternion& other) const{
            NQuaternion quaternion;
            quaternion.x = ((x * other.w) + (other.x * w)) + ((y * other.z) - (z * other.y));
            quaternion.y = ((y * other.w) + (other.y * w)) + ((z * other.x) - (x * other.z));
            quaternion.z = ((z * other.w) + (other.z * w)) + ((x * other.y) - (y * other.x));
            quaternion.w = (w * other.w) - (((x * other.x) + (y * other.y)) + (z * other.z));
            return quaternion;
        }

        NQuaternion& operator =(const NQuaternion& other){
            x = other.x;
            y = other.y;
            z = other.z;
            w = other.w;
            return *this;
        }

        void setAngleAxis(const T& angle, const NVector3<T>& axis){
            T theta = angle / static_cast<T>(2);
            T s = static_cast<T>(sin(theta) / Length(axis));
            T c = static_cast<T>(cos(theta));
            x = axis.x * s;
            y = axis.y * s;
            z = axis.z * s;
            w = c;
        }
		T Norm() const
		{
			return x*x + y*y + z*z + w*w;
		}

		T Mod() const
		{
			return static_cast<T>(sqrt(x*x + y*y + z*z + w*w));
		}

        void inverse(){
            T n = -1 / Norm();
            x *= n;
            y *= n;
            z *= n;
            w *= -n;
        }

        void conjugate(){
            x = -x;
            y = -y;
            z = -z;
        }

        void negate(){
            T nim = -1 / Mod();
            x *= nim;
            y *= nim;
            z *= nim;
            w *= -nim;
        }
    };

    template <typename T>
    inline T dot(const NQuaternion<T>& a, const NQuaternion<T>& b){
        return a.x*b.x + a.y*b.y + a.z*b.z + a.w*b.w;
    }

    template <typename T>
    inline NQuaternion<T> lerp(const NQuaternion<T>& a, const NQuaternion<T>& b, const T& time){
        return NQuaternion<T>(
            a.x + (b.x - a.x) * time,
            a.y + (b.y - a.y) * time,
            a.z + (b.z - a.z) * time,
            a.w + (b.w - a.w) * time);
    }


    template <typename T>
	inline NQuaternion<T> slerp(const NQuaternion<T>& a, const NQuaternion<T>& b, const T& time){
		//AART - Advanced Animation and Rendering Techniques
		T cosom = a.x*b.x + a.y*b.y + a.z*b.z + a.w*b.w;
		if ( (1 + cosom) > std::numeric_limits<T>::epsilon() ){
			T sp;
			T sq;
			if ( (1 - cosom) > std::numeric_limits<T>::epsilon() ){
				double omega = acos(cosom);
				double sinom = 1.0 / std::sin(omega);

				sp = static_cast<T>(sin((1 - time) * omega) * sinom);
				sq = static_cast<T>(sin(time * omega) * sinom);
			}
			else{
				sp = 1 - time;
				sq = time;
			}
			return NQuaternion<T>(
				a.x*sp + b.x*sq,
				a.y*sp + b.y*sq,
				a.z*sp + b.z*sq,
				a.w*sp + b.w*sq);
		}
		else{
			T halfpi = static_cast<T>(N_PI / 2);
			T sp = static_cast<T>(sin((1 - time) * halfpi));
			T sq = static_cast<T>(sin(time * halfpi));
			return NQuaternion<T>(
				a.x*sp - a.y*sq,
				a.y*sp + a.x*sq,
				a.z*sp - a.w*sq,
				a.z);
		}
	}
}
#endif // NQuaternion_h__
